from fileinput import filename

import pandas as pd
from matplotlib import patches

import DrawModel



t = DrawModel.Draw(length=60, width=30)
t.ax.set_xlim(8 + 16, t.road_length)

# t.ax.set_ylim(0, t.road_length)
max_hight = t.road_width * 6
road_1 = patches.Rectangle(
            (0, max_hight),  # 左下角坐标
            t.road_length,  # 矩形长度（x 方向）
            t.road_width,  # 矩形宽度（y 方向）
            facecolor='gray',
            edgecolor='black'
        )
t.ax.add_patch(road_1)
line_hight = [max_hight + t.road_width]
for i in range(0, 5):
    line_hight.append(max_hight - i * t.road_width)

print(line_hight)
for i in line_hight:
    t.drawLine(i, color="black", linewidth=8, linestyle = "-")

t.drawLine(max_hight + t.road_width / 2)

for i in range(1, 5):
    road_ = patches.Rectangle(
                (0, line_hight[-i]),  # 左下角坐标
                t.road_length,  # 矩形长度（x 方向）
                t.road_width / 2,  # 矩形宽度（y 方向）
                facecolor='gray',
                edgecolor='white'
            )
    t.ax.add_patch(road_)
    t.drawLine(line_hight[-i] + t.road_width / 2)

for z in range(1, 6):
    filename = f"../trajectory/test{str(z)}.csv"
    df = pd.read_csv(filename)
    npc_data = df.to_numpy()
    x = npc_data[:, 0]
    y = npc_data[:, 1] + (7 - (z - 0)) * t.road_width
    h = npc_data[:, 3]
    for i in range(0, len(x)):

        colors = (0.5, 0.2, i / len(x), 0.5)
        t.drawCarOutline(begin_x=float(x[i]), begin_y=float(y[i]), heading=float(h[i]), color=colors)
        # t.drawText(float(x[i] + 0.5), float(y[i]), str(i), 'white')
    t.drawCar(begin_x=float(x[0]), begin_y=float(y[0]), heading=float(h[0]), car_name=f"Car{str(z)}")


t.savefig("car1_5")






